int cmpfunc(const void *a,const void *b) {
    double *x = (double *) a;
    double *y = (double *) b;

    if (*x < *y) return -1;
    else if (*x > *y) return 1; 
    else return 0;
}
// One-dimensional linear interpolation
void LinInterp(double *Yi, double *X, double *Y, double *Xi,int dimX,int dimXi)
{
    int i,k=1;
    for(i=0; i<dimXi;i++){      // Loop over nodes in Xi
        k = 1;      // RESET!
        if (Xi[i]>=X[dimX-1])   // Above --> linear extrapolation
            Yi[i] = Y[dimX-1] + (Xi[i]-X[dimX-1])*(Y[dimX-1]-Y[dimX-1-1])/(X[dimX-1]-X[dimX-1-1]);
        else if (Xi[i]<=X[0])   // Below --> linear extrapolation
            Yi[i] = Y[0] + (Xi[i]-X[0])*(Y[1]-Y[0])/(X[1]-X[0]);
        else {
            // Find the k's (nearest observed X from left)
            while(k<dimX&&Xi[i]>X[k]){  k++; }
            k=k-1;  // k is the point in X to the left of Xi
            Yi[i] = Y[k] + (Xi[i]-X[k])*(Y[k+1]-Y[k])/(X[k+1]-X[k]); // Output
        }
    }
}

void UpperEnvelope(double *uM,double *uC,double *uVt,double *M,double *C,double *Vt,int dim)
{
// INPUTS: double *M,double *C,double *Vt,int dim,double *Common
//         dim: number of grid points of m 
// OUTPUTS: double *uM,double *uC,double *uVt
	
	for(int i=0;i<dim;i++){
            uM[i] = M[i];
            uC[i] = C[i];
            uVt[i]= Vt[i];
    }
	
    int Fall[MAXKINKS], Increase[MAXKINKS];
    double Cross[MAXKINKS];
    int NumKinks = 0;
    int iB,lo,hi,xL,xU;
    double Mcom[MAXCOM],V1[MAXCOM],V2[MAXCOM], C1[MAXCOM],C2[MAXCOM];
    double D1,D2;
    int NumMcom,i_com;
    int kink,lower,upper;
    int i = 1; // Start at one because I check one index "back"
    while(i<=(dim-1)){ 
        
        // ... first check if value decrease in current i 
        if (Vt[i]<Vt[i-1] && M[i]>M[i-1]) {

			int i0, i1; 
			if (i+1<dim && Vt[i+1]>Vt[i-1] && M[i+1]>M[i]){ // interpolation using Vt[i-1] and Vt[i+1]
				i0 = i-1; 
				i1 = i+1; 				
			} else { // interpolation using Vt[i-1] and Vt[i-2]
				if (i-2<0) {printf("\n Errors in Uppper(): please increases NUMVif !!!!: i=1: v[1] %f <v[0] %f \n exiting ...\n", Vt[i], Vt[i-1]); exit(1); } 			
				i0 = i-2; 
				i1 = i-1; 
			}	
			
			double vslope = (Vt[i1] - Vt[i0])/(M[i1] - M[i0]); 
			double cslope = (C[i1] - C[i0])  /(M[i1] - M[i0]); 
			if (cslope < 0.0) cslope = 0.0; 			
			
			uM[i]  = M[i];  
        	uVt[i] = Vt[i0] + (uM[i] - M[i0]) * vslope;             		
        	uC[i]  = C[i0] +  (uM[i] - M[i0]) * cslope; 
    		
			M[i]   = uM[i]; 
        	Vt[i]  = uVt[i]; 
            C[i]   = uC[i]; 
			
		} // value fall at i: Vt[i]<Vt[i-1] && M[i]>M[i-1]
        
        // ... next check if resources fall in i+1
        if ((M[i+1]<=M[i] && M[i]>M[i-1]) && i<(dim-1)){ 
		
            NumKinks++;
            if (NumKinks>MAXKINKS) { printf("Error in UpperEnvelope.c: Too many kinks! Update MAXKINKS"); exit(1); }
            Fall[NumKinks-1] = i; // Insert index for the current fall in resources
            
            // Find where resources increases again: B
            iB = i;
            while (M[iB+1]<=M[iB]){
                iB++;
            }
            Increase[NumKinks-1] = iB;
			
			// here: M[hi] should > M[i])
            // Find the upper bound on the non-concave region: D
            hi = iB;
            while(hi<dim && M[hi]<=M[i]){
                hi++;
            }
			
			// here: M[lo] should < than the smallest points between (M[iB] and M[hi])
            // Find the lower bound on the non-concave region: C
            lo = i;
            while(lo>=0 && M[lo]>=M[iB]) {
                lo--;
            }
 			
			if ( lo<0 || iB >= dim || hi >= dim ){ printf("\n Errors in Upper(): i %d (M %f, Vt %f), iB %d (%f, %f), hi %d (%f, %f), lo %d (%f, %f),", i, M[i], Vt[i], iB, M[iB], Vt[iB], hi, M[hi], Vt[hi], lo, M[lo], Vt[lo] ); exit(1); }
			
			#ifdef DEBUG
			// check to make sure: M[lo] < M[i], M[lo] <= M[iB], M[hi] > M[i]
			if ( M[lo] >= M[iB] || M[hi] <= M[i] ){ printf("\n Errors in Upper(): i %d (M %f, Vt %f), iB %d (%f, %f), hi %d (%f, %f), lo %d (%f, %f),", i, M[i], Vt[i], iB, M[iB], Vt[iB], hi, M[hi], Vt[hi], lo, M[lo], Vt[lo] ); exit(1); }
			#endif 
			
			// finding the crossing points      
			// Find the point just BEFORE and AFTER the intersection
			// First interpolate on common grid from m[lo] to m[hi]
			NumMcom = hi - lo +1; // i-lo+1 + hi-iB+1;
			if (NumMcom>MAXCOM) { printf("Error in UpperEnvelope.c: Too many points in common grid! Update MAXCOM"); exit(1); }
			for(i_com=0;i_com<NumMcom;i_com++){
    			Mcom[i_com] = M[lo+i_com]; 
			}
			qsort(&Mcom[0], NumMcom, sizeof(double), cmpfunc); 
			LinInterp(&V1[0],&M[lo],&Vt[lo],&Mcom[0],i-lo+1,NumMcom);   
			LinInterp(&V2[0],&M[iB],&Vt[iB],&Mcom[0],hi-iB+1,NumMcom);  
 

            
			xL = NumMcom-1;
			while (xL>0 && V1[xL]<V2[xL]){ // This will give the point just to the right of the cross
    			xL--;
			}
			xU = xL+1; 
			
			
			
            if ( V1[xL] > V2[xL] && V1[xU] < V2[xU]  ) {
            
                D1 = (V1[xU]-V1[xL])/(Mcom[xU]-Mcom[xL]);
                D2 = (V2[xU]-V2[xL])/(Mcom[xU]-Mcom[xL]);
                
                Cross[NumKinks-1]  = Mcom[xL] + (V1[xL]-V2[xL])/(D2-D1); 
                
            	double vcross1 = V1[xL] + D1 * (Cross[NumKinks-1] - Mcom[xL]); 
            	
            	#ifdef DEBUG 
            	double vcross2 = V2[xL] + D2 * (Cross[NumKinks-1] - Mcom[xL]); 
            	if ( fabs(vcross1-vcross2) > 1.0e-10 ){ printf("\n Errors in Upper() , v1 %f, v2 %f \t Mcom[%d] %f v[%f, %f], Mcom[%d] %f v[%f, %f] \n exiting...", vcross1, vcross2, xL, Mcom[xL], V1[xL], V2[xL], xU, Mcom[xU], V1[xU], V2[xU]); exit(1); }
            	#endif 
            	
            	// I assign cross point to Mcom[xL]
            	Mcom[xL] = Cross[NumKinks-1]; 
            	V1[xL]   = vcross1;
				V2[xL]   = vcross1; 
			      	
            }  else { // No cross has been found: set it to the point on the upper bound of the non-concave region
            	
            	Cross[NumKinks-1] = M[hi];
            }  

 			LinInterp(&C1[0],&M[lo],&C[lo],&Mcom[0],i-lo+1,NumMcom);   
			LinInterp(&C2[0],&M[iB],&C[iB],&Mcom[0],hi-iB+1,NumMcom);  
			
 
			
 
            for (int ii = lo+1; ii < hi; ii++){
            	
			
            	i_com = ii-lo; 
            	
            	uM[ii] = Mcom[i_com]; 

            	
            	if (V1[i_com] >= V2[i_com]){
            		
            		uVt[ii] = V1[i_com]; 
            		
            		uC[ii]  = C1[i_com]; 
            		

					
            	} else {
     			   	
            		uVt[ii] = V2[i_com]; 
            		
            		uC[ii]  = C2[i_com]; 
            		

            	}


            	
            	M[ii]   = uM[ii]; 
            	Vt[ii]  = uVt[ii] ; 
            	C[ii]   = uC[ii]; 
            	
            }
		    
            // for speed
        //    i = iB; 
            
        } // resources will fall at i+1: (M[i+1]<=M[i] && M[i]>M[i-1])
		        
        i++;
    
    } // loop over index

}
